#include <ros/ros.h>
#include <diffbot_base/diffbot_hw_interface.h>
#include <controller_manager/controller_manager.h>
 
int main(int argc, char **argv)
{
    // 初始化 ROS 节点，节点名为 "diffbot_hw_interface"
    ros::init(argc, argv, "diffbot_hw_interface");
    ros::NodeHandle nh;
    
    // 创建机器人实例 diffBot，使该实例能够访问所有可用的硬件资源
    diffbot_base::DiffBotHWInterface diffBot(nh);
 
    // 创建控制器管理器实例，并将机器人实例传递给它
    // 这样控制器管理器就能够管理机器人的硬件资源
    controller_manager::ControllerManager cm(&diffBot);
    
    // 设置一个独立的线程来处理 ROS 的回调函数
    // 注意：我们将 ROS 循环运行在一个独立的线程中，
    // 以避免外部调用（例如加载控制器的服务回调）阻塞主控制循环
    ros::AsyncSpinner spinner(1);
    spinner.start();
    
    // 设置控制循环
    ros::Time prev_time = ros::Time::now();  // 获取当前时间
    ros::Rate rate(10.0);  // 控制循环频率设置为 10 Hz
    rate.sleep();  // 休眠，确保第一次循环与速率一致

    // 当 ros::ok() 为 true 时，循环继续执行，直到收到关闭信号
    while (ros::ok())
    {
        // 获取当前时间并计算控制周期
        const ros::Time     time   = ros::Time::now();
        const ros::Duration period = time - prev_time;  // 计算从上一轮到当前的时间间隔
        prev_time = time;

        // 执行控制循环中的实际操作
        // 读取机器人当前状态
        diffBot.read(time, period);

        // 如果需要，可以通过调用以下函数定义软件中的传动：
        // transmission_interface::ActuatorToJointPositionInterface::propagate()
        // 来在读取关节状态后执行转换。
        cm.update(time, period);  // 更新控制器的状态，处理控制器的输入和输出

        // 在使用软件传动时，通过以下函数进行关节空间到执行器空间的转换：
        // transmission_interface::JointToActuatorEffortHandle::propagate()
        diffBot.write(time, period);  // 将控制信号发送给硬件

        // 重复这些步骤，按照指定的频率进行
        rate.sleep();  // 控制频率，保证循环时间间隔为设定的周期
    }

    return 0;
}
